/*!
  ******************************************************************************
  * @file           : telemetry.cpp
  * @author         :He  Kunling
  * @createDate     : 2021/07/31
  * @lastEditors    :
  * @lastEditTime   : 2021/08/01
  * @brief          : 
  * 
  *     This file provides functions bellow to be called:
  * 
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2020 Zero_One NUAA.
  * All rights reserved.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "main.hpp"

/* Namespace -----------------------------------------------------------------*/
using namespace std;


uint64_t  micros(void)
{
        struct timeval time_now;
        uint64_t usec_retval;
        gettimeofday(&time_now, NULL);

         usec_retval = 1000000L * time_now.tv_sec + time_now.tv_usec;

        return usec_retval;
}



void* telemetry_thread(void* arg)
{
    uint8_t loop_1Hz  = 0;
    uint8_t loop_5Hz  = 0;
    uint8_t loop_10Hz = 0;
    uint8_t loop_20Hz = 0;
    uint8_t loop_50Hz = 0;
    uint8_t loop_100Hz = 0;
    uint8_t loop_endDev = 0;
    uint64_t time_5ms = 0;
    uint64_t time_error = 0;
    uint8_t query_loop = 0;     //用于轮询飞控数据
    usr_thread_t *temp = (usr_thread_t*)arg;

    uint8_t flightcontrol_rcv_buf[128];


    while(1)
    {
        NX_serial_read_flight_control_raw_data(temp->usr_flightcontrol, &flightcontrol_ringbuf);  //  flightcontrol_ringbuf 环形缓冲区
        NX_analyse_flight_control_data(&flightcontrol_ringbuf, flightcontrol_rcv_buf);     //环形缓冲区       

        if((micros()-time_5ms)>=5000)
        {
            time_5ms = micros();
            loop_20Hz++;
            loop_50Hz++;
            loop_10Hz++;
            loop_5Hz++;
            loop_1Hz++;	         
        }
        if(loop_50Hz>=4){
            loop_50Hz = 0;

            query_loop++;
            switch (query_loop)
            {               
                case 1:
                   groundbase_cmd.query_cmd = 0x0A;     //查询包1的查询ID
                    NX_serial_send_to_flight_control_query(temp->usr_flightcontrol,&groundbase_cmd);  //查询指令
                    break;
            
                case 2:
                    groundbase_cmd.query_cmd = 0x0B;    //查询包2的查询ID
                    NX_serial_send_to_flight_control_query(temp->usr_flightcontrol,&groundbase_cmd);  //查询指令
                    break;

                case 3:
                    groundbase_cmd.query_cmd = 0x0C;    //查询包3的查询ID
                    NX_serial_send_to_flight_control_query(temp->usr_flightcontrol,&groundbase_cmd);  //查询指令
                    break;

                case 4:
                    //groundbase_cmd.query_cmd = 0x0D;    //查询包4的查询ID  修改成航点查询包
               //  groundbase_cmd.query_cmd = 0x08;
              //   NX_serial_send_to_flight_control_query(temp->usr_flightcontrol,&groundbase_cmd);  //查询指令
                    break;
            
                case 5:
                    groundbase_cmd.query_cmd = 0x10;    //查询包7的查询ID
                    NX_serial_send_to_flight_control_query(temp->usr_flightcontrol,&groundbase_cmd);  //查询指令
                    break;
            
                case 6:
                    groundbase_cmd.query_cmd = 0x11;    //查询包8的查询ID
                    NX_serial_send_to_flight_control_query(temp->usr_flightcontrol,&groundbase_cmd);  //查询指令
                    query_loop = 0;      //轮询清0
                    break;
 
                default:
                    break;
            }
        }

        if(loop_20Hz>=10){
            loop_20Hz = 0;
            NX_send_waypoint_to_flight_control( temp->usr_flightcontrol);
            NX_serial_send_to_flight_control(temp->usr_flightcontrol,&groundbase_cmd);  //控制指令
            if(NX_get_up_waypoint_single_flag)  ///向飞控发送请求单个航点信息的查询包
           {  
               groundbase_cmd.query_cmd = 0x08;  
               NX_serial_send_to_flight_control_query(temp->usr_flightcontrol,&groundbase_cmd);  //查询指令
           }
            if(usr_flag.onboard_land_flag)
            {
                IARC_task(&groundbase_cmd);
                printf("start task\n");
            }

        }
        
        if(loop_10Hz>=20){
            loop_10Hz = 0;          
        }
        if(loop_5Hz>=40){
            loop_5Hz = 0;
        }

        if(loop_1Hz>=200){
            loop_1Hz = 0;

            std::cout << " 我是飞控线程：" << std::endl;
            
            if(usr_flag.control_flag == true)
            {
                std::cout << " ~指 令 执 行 成 功~" << std::endl;
            }


            if(usr_flag.query_1 && usr_flag.query_7 && usr_flag.query_8) 
            {
                flight_control_lose_massage_cnt = 0;
                flight_control_lose_massage_flag = false;   
            }

            if(usr_flag.query_1 == true)
            {
               std::cout << " ~查 询 包 1 接 收 成 功~" << std::endl;
                usr_flag.query_1 = false;
            }else{
                flight_control_lose_massage_cnt++;
            }
            
            if(usr_flag.query_2 == true)
            {
               std::cout << " ~查 询 包 2 接 收 成 功~" << std::endl;
                usr_flag.query_2 = false;
            }else{
            }
                
            if(usr_flag.query_3 == true)
            {
               std::cout << " ~查 询 包 3 接 收 成 功~" << std::endl;
                usr_flag.query_3 = false;
            }else{
            }
                
            if(usr_flag.query_4 == true)
            {
               std::cout << " ~查 询 包 4 接 收 成 功~" << std::endl;
                usr_flag.query_4 = false;
            }else{
            }
                

            if(usr_flag.query_7 == true)
            {
               std::cout << " ~查 询 包 7 接 收 成 功~" << std::endl;
                usr_flag.query_7 = false;
            }else{
                flight_control_lose_massage_cnt++;
            }
                
            if(usr_flag.query_8 == true)
            {
                std::cout << " ~查 询 包 8 接 收 成 功~" << std::endl;
                usr_flag.query_8 = false;
            }else{
                flight_control_lose_massage_cnt++;
            }

            if(flight_control_lose_massage_cnt >= 1)
            {
                flight_control_lose_massage_flag = true;
            }


              printf("flight_control_lose_massage_cnt =%d\n",flight_control_lose_massage_cnt );

             
        }
        
    }

}

